import os
import sys

sys.path.append(".")

import math
import numpy as np
# import converter
import matplotlib
matplotlib.use('TkAgg')
from matplotlib import pyplot as plt
from matplotlib.lines import Line2D
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Line3D
import time_processor as tp
import geo_processor as gp
import math_processor as mp
from scipy import interpolate

def read_tum_data(tum_path):

    result = []

    num = 0
    with open(tum_path) as f:
        for line in f:
            if line[0] != '#':
                pass
            num = num + 1 
            items = line.split(' ')
    
            result1 = []
            result1.append(float(items[0])) # timestamp
            result1.append(float(items[1])) # Px
            result1.append(float(items[2])) # Py
            result1.append(float(items[3])) # Pz

            euler = mp.quat2euler([float(items[4]), float(items[5]), float(items[6]), float(items[7])])

            result1.append(float(euler[0])) # Rx
            result1.append(float(euler[1])) # Ry
            result1.append(float(euler[2])) # Rz

            result.append(result1)

    return np.array(result)


def calcuate_r_ic(tum1, tum2):
    
    fx = interpolate.interp1d(tum1[:,0], tum1[:,4])
    fy = interpolate.interp1d(tum1[:,0], tum1[:,5])
    fz = interpolate.interp1d(tum1[:,0], tum1[:,6])

    for i in range(tum2.shape[0]):

        xnew = fx(tum2[i,0])
        ynew = fy(tum2[i,0])
        znew = fz(tum2[i,0])

        rot_w_i = mp.euler2rot([xnew, ynew, znew])
        rot_w_c = mp.euler2rot([tum2[i,4], tum2[i,5], tum2[i,6]])
        rot_i_c = np.matmul(rot_w_i.transpose(), rot_w_c)

        euler_i_c = mp.rot2euler(rot_i_c)
        print(euler_i_c)

if __name__ == "__main__":

    path1 = r"/shared/VIO/openvslam-main/build/trajectory_gt.txt"
    path2 = r"/shared/HBKit/frame_trajectory_new.tum"
    tum1 = read_tum_data(path1)
    tum2 = read_tum_data(path2)
    calcuate_r_ic(tum1, tum2)
    
